#!/usr/bin/env python3

import rospy
import paho.mqtt.client as mqtt
from std_msgs.msg import String
import struct

class MqttBridge(object):
    yaw_offset = 0
    def __init__(self):
        self.mqtt_client = mqtt.Client()
        self.mqtt_client.on_connect = self.on_connect
        self.mqtt_client.on_disconnect = self.on_disconnect
        self.mqtt_client.connect("test.mosquitto.org", 1883, 60)
        self.mqtt_client.loop_start()

        self.gps_publisher = rospy.Publisher('/gps', String, queue_size=10)

    def on_connect(self, client, userdata, flags, rc):
        rospy.logwarn("[pub_test]Connected to MQTT broker with result code %s", str(rc))
        client.subscribe("wpr_gps")

    def on_disconnect(self, client, userdata, rc):
        rospy.logwarn("[pub_test] Disconnected from MQTT broker with result code %s", str(rc))

    def run(self):
        latitude = 39.74025 + 0.0125
        longitude = 116.12685 - 0.0128
        yaw = 45
        data = ",".join([str(latitude),str(longitude),str(yaw)])
        while not rospy.is_shutdown():
            # 向MQTT网络发布数据
            self.mqtt_client.publish("wpr_robot_gps", data)
            rospy.sleep(1)

if __name__ == '__main__':
    rospy.init_node('pub_test')
    mqtt_bridge = MqttBridge()
    mqtt_bridge.run()
